#pragma once

#include <boost/interprocess/mapped_region.hpp>
#include <boost/interprocess/shared_memory_object.hpp>
#include <boost/interprocess/sync/named_mutex.hpp>
#include <memory>
#include <ros/ros.h>
#include <ros/serialization.h>
#include <utility>

using namespace boost::interprocess;
namespace Serializer = ros::serialization;

using ShareMemory       = boost::interprocess::shared_memory_object;
using ShareMemoryRegion = boost::interprocess::mapped_region;
using ShareMemoryMutex  = boost::interprocess::named_mutex;

template < typename MessageType > class SharedMemoryROSCommunicator {
public:
    explicit SharedMemoryROSCommunicator( std::string channel, std::size_t memory_size = 1024 )
        : channel_( std::move( channel ) ), memory_size_( memory_size ) {
        try {
            creator_         = false;
            sharedMemoryPtr_ = std::make_unique< ShareMemory >( open_only, channel_.c_str(), read_write );
        }
        catch ( const boost::interprocess::interprocess_exception& ex ) {
            creator_         = true;
            sharedMemoryPtr_ = std::make_unique< ShareMemory >( open_or_create, channel_.c_str(), read_write );
        }
        sharedMemoryPtr_->truncate( memory_size_ );
        regionPtr_           = std::make_unique< ShareMemoryRegion >( *sharedMemoryPtr_, read_write );
        std::string mux_name = channel_ + "_mux";  // mutex name
        mutexPtr_            = std::make_unique< ShareMemoryMutex >( open_or_create, mux_name.c_str() );
    }

    void publish( const MessageType& msg ) {
        uint32_t                       buf_size = Serializer::serializationLength( msg );
        boost::shared_array< uint8_t > buffer( new uint8_t[ buf_size ] );
        Serializer::OStream            stream( buffer.get(), buf_size );
        Serializer::serialize( stream, msg );
        mutexPtr_->lock();
        memcpy( regionPtr_->get_address(), buffer.get(), buf_size );
        mutexPtr_->unlock();
    }

    MessageType read() {
        MessageType                    msg;
        boost::shared_array< uint8_t > buffer( new uint8_t[ memory_size_ ] );
        mutexPtr_->lock();
        memcpy( buffer.get(), regionPtr_->get_address(), memory_size_ );
        mutexPtr_->unlock();
        Serializer::IStream stream( buffer.get(), memory_size_ );
        Serializer::deserialize( stream, msg );

        return msg;
    }

    ~SharedMemoryROSCommunicator() {
        if ( creator_ ) {
            sharedMemoryPtr_->remove( channel_.c_str() );
            std::string mux_name = channel_ + "_mux";
            ShareMemoryMutex::remove( mux_name.c_str() );
        }
    }

private:
    std::string                          channel_;
    std::size_t                          memory_size_;
    bool                                 creator_;
    std::unique_ptr< ShareMemory >       sharedMemoryPtr_;
    std::unique_ptr< ShareMemoryRegion > regionPtr_;
    std::unique_ptr< ShareMemoryMutex >  mutexPtr_;
};
